clear all
m_dry = 1505;       % Vehicle mass without fuel [kg]
Isp = 225;          % Specific impulse [s]
g0 = 9.80665;       % Standard earth gravity [m/s^2]
g = [0 ; -3.7114];  % Mars gravity vector [m/s^2]
max_throttle = 0.9; % Max open throttle [.%]
min_throttle = 0.3; % Min open throttle [.%]
T_max = 6 * 3100;   % Max total thrust force at 1.0 throttle [N]
theta = 27;           % The cant angle of thrusters [deg]

% Initial conditions
m_wet = 1905;           % Vehicle mass with fuel [kg]
r0 = [  2 ; 1.5] * 1e3; % Initial position [x;z] [m]
v0 = [100 ; -75];       % Initial velocity [x;z] [m/s]

% Target conditions
rf = [ 0 ; 0 ];
vf = [ 0 ; 0 ];

% tf = 75;  % Target end time [s]
% dt = 1.0; % Discrete node time interval [s]
% N = (tf / dt) + 1;
% tv = 0:dt:tf;
% Inputs:
% N     : Number of knot points in discrete optimization problem
% r0    : Initial position [m]   |  rf : Final position [m]
% v0    : Initial velocity [m/s] |  vf : Final velocity [m/s]
% m_wet : Initial total mass [kg]
% theta : Thrust pointing limit from vertical [deg]
% p : Vehicle/planet parameters
% p.m_dry        : Vehicle mass without fuel [kg]
% p.Isp          : Specific impulse [s]
% p.g            : Planet gravity vector [m/s^2]
% p.max_throttle : Max open throttle [.%]
% p.min_throttle : Min open throttle [.%]
% p.T_max        : Max total thrust force at 1.0 throttle [N]
% p.phi          : The cant angle of thrusters [deg]
%
% Outputs:
% tv     : Vector (1xN) of time at knot points [s]
% m_used : Fuel mass used [kg]
% r      : Position(t) [m]
% v      : Velocity(t) [m/s]
% u      : Command Acceleration(t) [m/s^2]
% m      : Mass(t) [kg]
p.m_dry = m_dry;
p.Isp = Isp;
p.g = g;
p.max_throttle = max_throttle;
p.min_throttle = min_throttle;
p.T_max = T_max;
p.phi = 15;
N = 50;
[tv,m_used,r,v,u,m,s,z,s_bound_min,s_bound_max,z_bound]=GFOLD(N,r0,v0,rf,vf,m_wet,theta,p);
% 判断不等式约束是否非紧约束
% alpha = 1 / (Isp * g0 * cosd(phi));
% r1 = min_throttle * T_max * cosd(phi);
% r2 = max_throttle * T_max * cosd(phi);
%% 紧约束计算
T_constraint = zeros(N,1);
S_constraint = zeros(N,2);
Z_constraint = zeros(N,2);
Point_constraint = zeros(N,1);
UnderFly_constraint = zeros(N,1);
u_min_bound = s .* cosd(theta);
for i = 1:1:N
    % 推力约束
    T_constraint(i,1) = my_equal(norm(u(:,i)),s(i));
    % 推力松弛变量约束
    S_constraint(i,1) = my_equal(s(i),s_bound_min(i));
    S_constraint(i,2) = my_equal(s(i),s_bound_max(i));
    % 物理质量上下界
    Z_constraint(i,1) = my_equal(z(i),z_bound(1));
    Z_constraint(i,2) = my_equal(z(i),z_bound(2));
    %推力指向约束
    Point_constraint(i,1) = my_equal(u(2,i),u_min_bound(1,i));
    
end



%地下飞行约束
for i = 1:1:N-1
    UnderFly_constraint(i,1) = my_equal(r(2,i),0);
end
%%  
make_video('res.avi',tv,r,v,u)
%% 
%创建视频播放对象
a = 'res.avi';
vid = VideoReader(a);
 
% % 获取视频的基本信息
% numFrames = vid.NumFrames;
% fps = vid.FrameRate;
% 
% % 初始化一个 figure
% figure;
% 
% for i = 1:numFrames
%     frame = read(vid, i);
%     imshow(frame);
%     pause(1/100); % 控制播放速度
% end
